<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN"
                "http://www.w3.org/TR/REC-html40/loose.dtd">
<html>
<head>
  <title>Description of ukf</title>
  <meta name="keywords" content="ukf">
  <meta name="description" content="UKF  Unscented Kalman Filter">
  <meta http-equiv="Content-Type" content="text/html; charset=iso-8859-1">
  <meta name="generator" content="m2html &copy; 2003 Guillaume Flandin">
  <meta name="robots" content="index, follow">
  <link type="text/css" rel="stylesheet" href="../../m2html.css">
</head>
<body>
<a name="_top"></a>
<div><a href="../../menu.html">Home</a> &gt;  <a href="#">ReBEL-0.2.7</a> &gt; <a href="#">core</a> &gt; ukf.m</div>

<!--<table width="100%"><tr><td align="left"><a href="../../menu.html"><img alt="<" border="0" src="../../left.png">&nbsp;Master index</a></td>
<td align="right"><a href="menu.html">Index for .\ReBEL-0.2.7\core&nbsp;<img alt=">" border="0" src="../../right.png"></a></td></tr></table>-->

<h1>ukf
</h1>

<h2><a name="_name"></a>PURPOSE <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="box"><strong>UKF  Unscented Kalman Filter</strong></div>

<h2><a name="_synopsis"></a>SYNOPSIS <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="box"><strong>function [xh, Px, pNoise, oNoise, InternalVariablesDS] = ukf(state, Pstate, pNoise, oNoise, obs, U1, U2, InferenceDS) </strong></div>

<h2><a name="_description"></a>DESCRIPTION <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="fragment"><pre class="comment"> UKF  Unscented Kalman Filter

   [xh, Px, pNoise, oNoise, InternalVariablesDS] = ukf(state, Pstate, pNoise, oNoise, obs, U1, U2, InferenceDS)

   This filter assumes the following standard state-space model:

     x(k) = ffun[x(k-1),v(k-1),U1(k-1)]
     y(k) = hfun[x(k),n(k),U2(k)]

   where x is the system state, v the process noise, n the observation noise, u1 the exogenous input to the state
   transition function, u2 the exogenous input to the state observation function and y the noisy observation of the
   system.

   INPUT
         state                state mean at time k-1          ( xh(k-1) )
         Pstate               state covariance at time k-1    ( Px(k-1) )
         pNoise               process noise data structure     (must be of type 'gaussian' or 'combo-gaussian')
         oNoise               observation noise data structure (must be of type 'gaussian' or 'combo-gaussian')
         obs                  noisy observations starting at time k ( y(k),y(k+1),...,y(k+N-1) )
         U1                   exogenous input to state transition function starting at time k-1 ( u1(k-1),u1(k),...,u1(k+N-2) )
         U2                   exogenous input to state observation function starting at time k  ( u2(k),u2(k+1),...,u2(k+N-1) )
         InferenceDS          inference data structure generated by GENINFDS function.

   OUTPUT
         xh                   estimates of state starting at time k ( E[x(t)|y(1),y(2),...,y(t)] for t=k,k+1,...,k+N-1 )
         Px                   state covariance
         pNoise               process noise data structure     (possibly updated)
         oNoise               observation noise data structure (possibly updated)

         InternalVariablesDS  (optional) internal variables data structure
           .xh_                  predicted state mean ( E[x(t)|y(1),y(2),..y(t-1)] for t=k,k+1,...,k+N-1 )
           .Px_                  predicted state covariance
           .yh_                  predicted observation ( E[y(k)|Y(k-1)] )
           .inov                 inovation signal
           .Pinov                inovation covariance
           .KG                   Kalman gain

   Required InferenceDS fields:
         .spkfParams           SPKF parameters = [alpha beta kappa] with
                                   alpha  :  UKF scale factor
                                   beta   :  UKF covariance correction factor
                                   kappa  :  UKF secondary scaling parameter
   Copyright (c) Oregon Health &amp; Science University (2006)

   This file is part of the ReBEL Toolkit. The ReBEL Toolkit is available free for
   academic use only (see included license file) and can be obtained from
   http://choosh.csee.ogi.edu/rebel/.  Businesses wishing to obtain a copy of the
   software should contact rebel@csee.ogi.edu for commercial licensing information.

   See LICENSE (which should be part of the main toolkit distribution) for more
   detail.</pre></div>

<!-- crossreference -->
<h2><a name="_cross"></a>CROSS-REFERENCE INFORMATION <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
This function calls:
<ul style="list-style-image:url(../../matlabicon.gif)">
<li><a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>	ADDANGLE   Addition function for 'angle space' sigma-points expressed in radians.</li><li><a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>	CVECREP  Column vector replicate</li><li><a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>	ADDANGLE   Addition function for 'angle space' sigma-points expressed in radians.</li></ul>
This function is called by:
<ul style="list-style-image:url(../../matlabicon.gif)">
<li><a href="../.././ReBEL-0.2.7/examples/parameter_estimation/dempe1.html" class="code" title="">dempe1</a>	DEMPE1  Demonstrate parameter estimation on a simple 2nd order LTI system.</li><li><a href="../.././ReBEL-0.2.7/examples/state_estimation/demse1.html" class="code" title="">demse1</a>	DEMSE1  Demonstrate state estimation on a simple 2nd order LTI system.</li><li><a href="../.././ReBEL-0.2.7/examples/state_estimation/demse2.html" class="code" title="">demse2</a>	DEMSE2  Demonstrate state estimation on a simple scalar nonlinear (time variant) problem</li><li><a href="../.././ReBEL-0.2.7/examples/state_estimation/demse3.html" class="code" title="">demse3</a>	DEMSE3  Demonstrate nonlinear time series state estimation for Mackey-Glass chaotic time series</li></ul>
<!-- crossreference -->


<h2><a name="_source"></a>SOURCE CODE <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="fragment"><pre>0001 <a name="_sub0" href="#_subfunctions" class="code">function [xh, Px, pNoise, oNoise, InternalVariablesDS] = ukf(state, Pstate, pNoise, oNoise, obs, U1, U2, InferenceDS)</a>
0002 
0003 <span class="comment">% UKF  Unscented Kalman Filter</span>
0004 <span class="comment">%</span>
0005 <span class="comment">%   [xh, Px, pNoise, oNoise, InternalVariablesDS] = ukf(state, Pstate, pNoise, oNoise, obs, U1, U2, InferenceDS)</span>
0006 <span class="comment">%</span>
0007 <span class="comment">%   This filter assumes the following standard state-space model:</span>
0008 <span class="comment">%</span>
0009 <span class="comment">%     x(k) = ffun[x(k-1),v(k-1),U1(k-1)]</span>
0010 <span class="comment">%     y(k) = hfun[x(k),n(k),U2(k)]</span>
0011 <span class="comment">%</span>
0012 <span class="comment">%   where x is the system state, v the process noise, n the observation noise, u1 the exogenous input to the state</span>
0013 <span class="comment">%   transition function, u2 the exogenous input to the state observation function and y the noisy observation of the</span>
0014 <span class="comment">%   system.</span>
0015 <span class="comment">%</span>
0016 <span class="comment">%   INPUT</span>
0017 <span class="comment">%         state                state mean at time k-1          ( xh(k-1) )</span>
0018 <span class="comment">%         Pstate               state covariance at time k-1    ( Px(k-1) )</span>
0019 <span class="comment">%         pNoise               process noise data structure     (must be of type 'gaussian' or 'combo-gaussian')</span>
0020 <span class="comment">%         oNoise               observation noise data structure (must be of type 'gaussian' or 'combo-gaussian')</span>
0021 <span class="comment">%         obs                  noisy observations starting at time k ( y(k),y(k+1),...,y(k+N-1) )</span>
0022 <span class="comment">%         U1                   exogenous input to state transition function starting at time k-1 ( u1(k-1),u1(k),...,u1(k+N-2) )</span>
0023 <span class="comment">%         U2                   exogenous input to state observation function starting at time k  ( u2(k),u2(k+1),...,u2(k+N-1) )</span>
0024 <span class="comment">%         InferenceDS          inference data structure generated by GENINFDS function.</span>
0025 <span class="comment">%</span>
0026 <span class="comment">%   OUTPUT</span>
0027 <span class="comment">%         xh                   estimates of state starting at time k ( E[x(t)|y(1),y(2),...,y(t)] for t=k,k+1,...,k+N-1 )</span>
0028 <span class="comment">%         Px                   state covariance</span>
0029 <span class="comment">%         pNoise               process noise data structure     (possibly updated)</span>
0030 <span class="comment">%         oNoise               observation noise data structure (possibly updated)</span>
0031 <span class="comment">%</span>
0032 <span class="comment">%         InternalVariablesDS  (optional) internal variables data structure</span>
0033 <span class="comment">%           .xh_                  predicted state mean ( E[x(t)|y(1),y(2),..y(t-1)] for t=k,k+1,...,k+N-1 )</span>
0034 <span class="comment">%           .Px_                  predicted state covariance</span>
0035 <span class="comment">%           .yh_                  predicted observation ( E[y(k)|Y(k-1)] )</span>
0036 <span class="comment">%           .inov                 inovation signal</span>
0037 <span class="comment">%           .Pinov                inovation covariance</span>
0038 <span class="comment">%           .KG                   Kalman gain</span>
0039 <span class="comment">%</span>
0040 <span class="comment">%   Required InferenceDS fields:</span>
0041 <span class="comment">%         .spkfParams           SPKF parameters = [alpha beta kappa] with</span>
0042 <span class="comment">%                                   alpha  :  UKF scale factor</span>
0043 <span class="comment">%                                   beta   :  UKF covariance correction factor</span>
0044 <span class="comment">%                                   kappa  :  UKF secondary scaling parameter</span>
0045 <span class="comment">%   Copyright (c) Oregon Health &amp; Science University (2006)</span>
0046 <span class="comment">%</span>
0047 <span class="comment">%   This file is part of the ReBEL Toolkit. The ReBEL Toolkit is available free for</span>
0048 <span class="comment">%   academic use only (see included license file) and can be obtained from</span>
0049 <span class="comment">%   http://choosh.csee.ogi.edu/rebel/.  Businesses wishing to obtain a copy of the</span>
0050 <span class="comment">%   software should contact rebel@csee.ogi.edu for commercial licensing information.</span>
0051 <span class="comment">%</span>
0052 <span class="comment">%   See LICENSE (which should be part of the main toolkit distribution) for more</span>
0053 <span class="comment">%   detail.</span>
0054 
0055 <span class="comment">%=============================================================================================</span>
0056 
0057 Xdim  = InferenceDS.statedim;                                <span class="comment">% extract state dimension</span>
0058 Odim  = InferenceDS.obsdim;                                  <span class="comment">% extract observation dimension</span>
0059 U1dim = InferenceDS.U1dim;                                   <span class="comment">% extract exogenous input 1 dimension</span>
0060 U2dim = InferenceDS.U2dim;                                   <span class="comment">% extract exogenous input 2 dimension</span>
0061 Vdim  = InferenceDS.Vdim;                                    <span class="comment">% extract process noise dimension</span>
0062 Ndim  = InferenceDS.Ndim;                                    <span class="comment">% extract observation noise dimension</span>
0063 
0064 NOV = size(obs,2);                                           <span class="comment">% number of input vectors</span>
0065 
0066 <span class="comment">%------------------------------------------------------------------------------------------------------------------</span>
0067 <span class="comment">%-- ERROR CHECKING</span>
0068 
0069 <span class="keyword">if</span> (nargin ~= 8) error(<span class="string">' [ ukf ] Not enough input arguments.'</span>); <span class="keyword">end</span>
0070 
0071 <span class="keyword">if</span> (Xdim~=size(state,1)) error(<span class="string">'[ ukf ] Prior state dimension differs from InferenceDS.statedim'</span>); <span class="keyword">end</span>
0072 <span class="keyword">if</span> (Xdim~=size(Pstate,1)) error(<span class="string">'[ ukf ] Prior state covariance dimension differs from InferenceDS.statedim'</span>); <span class="keyword">end</span>
0073 <span class="keyword">if</span> (Odim~=size(obs,1)) error(<span class="string">'[ ukf ] Observation dimension differs from InferenceDS.obsdim'</span>); <span class="keyword">end</span>
0074 <span class="keyword">if</span> U1dim
0075     [dim,nop] = size(U1);
0076     <span class="keyword">if</span> (U1dim~=dim) error(<span class="string">'[ ukf ] Exogenous input U1 dimension differs from InferenceDS.U1dim'</span>); <span class="keyword">end</span>
0077     <span class="keyword">if</span> (dim &amp; (NOV~=nop)) error(<span class="string">'[ ukf ] Number of observation vectors and number of exogenous input U1 vectors do not agree!'</span>); <span class="keyword">end</span>
0078 <span class="keyword">end</span>
0079 <span class="keyword">if</span> U2dim
0080     [dim,nop] = size(U2);
0081     <span class="keyword">if</span> (U2dim~=dim) error(<span class="string">'[ ukf ] Exogenous input U2 dimension differs from InferenceDS.U2dim'</span>); <span class="keyword">end</span>
0082     <span class="keyword">if</span> (dim &amp; (NOV~=nop)) error(<span class="string">'[ ukf ] Number of observation vectors and number of exogenous input U2 vectors do not agree!'</span>); <span class="keyword">end</span>
0083 <span class="keyword">end</span>
0084 
0085 xh   = zeros(Xdim,NOV);
0086 xh_  = zeros(Xdim,NOV);
0087 yh_  = zeros(Odim,NOV);
0088 inov = zeros(Odim,NOV);
0089 
0090 <span class="comment">%--------------------------------------------------------------------------------------------------------------------</span>
0091 
0092 <span class="comment">% Get UKF scaling parameters</span>
0093 alpha = InferenceDS.spkfParams(1);
0094 beta  = InferenceDS.spkfParams(2);
0095 kappa = InferenceDS.spkfParams(3);
0096 
0097 <span class="comment">% Get index vectors for any of the state or observation vector components that are angular quantities</span>
0098 <span class="comment">% which have discontinuities at +- Pi radians ?</span>
0099 
0100 sA_IdxVec = InferenceDS.stateAngleCompIdxVec;
0101 oA_IdxVec = InferenceDS.obsAngleCompIdxVec;
0102 
0103 L = Xdim + Vdim + Ndim;                                   <span class="comment">% augmented state dimension</span>
0104 nsp = 2*L+1;                                              <span class="comment">% number of sigma-points</span>
0105 kappa = alpha^2*(L+kappa)-L;                              <span class="comment">% compound scaling parameter</span>
0106 
0107 W = [kappa 0.5 0]/(L+kappa);                              <span class="comment">% sigma-point weights</span>
0108 W(3) = W(1) + (1-alpha^2) + beta;
0109 
0110 Sqrt_L_plus_kappa = sqrt(L+kappa);
0111 
0112 Zeros_Xdim_X_Vdim     = zeros(Xdim,Vdim);
0113 Zeros_Vdim_X_Xdim     = zeros(Vdim,Xdim);
0114 Zeros_XdimVdim_X_Ndim = zeros(Xdim+Vdim,Ndim);
0115 Zeros_Ndim_X_XdimVdim = zeros(Ndim,Xdim+Vdim);
0116 
0117 <span class="keyword">if</span> (U1dim==0), UU1=zeros(0,nsp); <span class="keyword">end</span>
0118 <span class="keyword">if</span> (U2dim==0), UU2=zeros(0,nsp); <span class="keyword">end</span>
0119 
0120 Sv = chol(pNoise.cov)';
0121 Sn = chol(oNoise.cov)';
0122 
0123 <span class="comment">%--------------------------------------- Loop over all input vectors --------------------------------------------</span>
0124 <span class="keyword">for</span> i=1:NOV,
0125 
0126     <span class="keyword">if</span> U1dim UU1 = <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(U1(:,i),nsp); <span class="keyword">end</span>
0127     <span class="keyword">if</span> U2dim UU2 = <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(U2(:,i),nsp); <span class="keyword">end</span>
0128 
0129     Sx = chol(Pstate)';
0130 
0131     <span class="comment">%------------------------------------------------------</span>
0132     <span class="comment">% TIME UPDATE</span>
0133 
0134     Z    = <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>([state; pNoise.mu; oNoise.mu], nsp);
0135     Zm   = Z;                                                   <span class="comment">% copy needed for possible angle components section</span>
0136     SzT  = [Sx Zeros_Xdim_X_Vdim; Zeros_Vdim_X_Xdim Sv];
0137     Sz   = [SzT Zeros_XdimVdim_X_Ndim; Zeros_Ndim_X_XdimVdim Sn];
0138     sSz  = Sqrt_L_plus_kappa * Sz;
0139     sSzM = [sSz -sSz];
0140     Z(:,2:nsp) = Z(:,2:nsp) + sSzM;           <span class="comment">% build sigma-point set</span>
0141 
0142 
0143     <span class="comment">%-- Calculate predicted state mean, dealing with angular discontinuities if needed</span>
0144     <span class="keyword">if</span> isempty(sA_IdxVec)
0145         X_ = InferenceDS.ffun( InferenceDS, Z(1:Xdim,:), Z(Xdim+1:Xdim+Vdim,:), UU1);  <span class="comment">% propagate sigma-points through process model</span>
0146         X_bps = X_;
0147         xh_(:,i) = W(1)*X_(:,1) + W(2)*sum(X_(:,2:nsp),2);
0148         temp1 = X_ - <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(xh_(:,i),nsp);
0149     <span class="keyword">else</span>
0150         Z(sA_IdxVec,2:nsp) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(Zm(sA_IdxVec,2:nsp), sSzM(sA_IdxVec,:));      <span class="comment">% fix sigma-point set for angular components</span>
0151         X_ = InferenceDS.ffun( InferenceDS, Z(1:Xdim,:), Z(Xdim+1:Xdim+Vdim,:), UU1); <span class="comment">% propagate sigma-points through process model</span>
0152         X_bps = X_;
0153         state_pivotA = X_(sA_IdxVec,1);                                <span class="comment">% extract pivot angle</span>
0154         X_(sA_IdxVec,1) = 0;
0155         X_(sA_IdxVec,2:end) = <a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>(X_(sA_IdxVec,2:end),<a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(state_pivotA,nsp-1));  <span class="comment">% subtract pivot angle mod 2pi</span>
0156         xh_(:,i) = W(1)*X_(:,1) + W(2)*sum(X_(:,2:nsp),2);
0157         xh_(sA_IdxVec,i) = 0;
0158         <span class="keyword">for</span> k=2:nsp,
0159           xh_(sA_IdxVec,i) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(xh_(sA_IdxVec,i), W(2)*X_(sA_IdxVec,k));     <span class="comment">% calculate UT mean ... mod 2pi</span>
0160         <span class="keyword">end</span>
0161         sFoo = <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(xh_(:,i),nsp);
0162         temp1 = X_ - sFoo;
0163         temp1(sA_IdxVec,:) = <a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>(X_(sA_IdxVec,:), sFoo(sA_IdxVec,:));
0164         xh_(sA_IdxVec,i) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(xh_(sA_IdxVec,i), state_pivotA);  <span class="comment">% add pivot angle back to calculate actual predicted mean</span>
0165     <span class="keyword">end</span>
0166 
0167     Px_ = W(3)*temp1(:,1)*temp1(:,1)' + W(2)*temp1(:,2:nsp)*temp1(:,2:nsp)';
0168 
0169     Y_ = InferenceDS.hfun( InferenceDS, X_bps, Z(Xdim+Vdim+1:Xdim+Vdim+Ndim,:), UU2);    <span class="comment">% propagate through observation model</span>
0170 
0171     <span class="comment">%-- Calculate predicted observation mean, dealing with angular discontinuities if needed</span>
0172     <span class="keyword">if</span> isempty(oA_IdxVec)
0173         yh_(:,i) = W(1)*Y_(:,1) + W(2)*sum(Y_(:,2:nsp),2);
0174         temp2 = Y_ - <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(yh_(:,i),nsp);
0175     <span class="keyword">else</span>
0176         obs_pivotA = Y_(oA_IdxVec,1);      <span class="comment">% extract pivot angle</span>
0177         Y_(oA_IdxVec,1) = 0;
0178         Y_(oA_IdxVec,2:end) = <a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>(Y_(oA_IdxVec,2:end),<a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(obs_pivotA,nsp-1));  <span class="comment">% subtract pivot angle mod 2pi</span>
0179         yh_(:,i) = W(1)*Y_(:,1) + W(2)*sum(Y_(:,2:nsp),2);
0180         yh_(oA_IdxVec,i) = 0;
0181         <span class="keyword">for</span> k=2:nsp,
0182           yh_(oA_IdxVec,i) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(yh_(oA_IdxVec,i), W(2)*Y_(oA_IdxVec,k));   <span class="comment">% calculate UT mean ... mod 2pi</span>
0183         <span class="keyword">end</span>
0184         oFoo = <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(yh_(:,i),nsp);
0185         temp2 = Y_ - oFoo;
0186         temp2(oA_IdxVec,:) = <a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>(Y_(oA_IdxVec,:), oFoo(oA_IdxVec,:));
0187         yh_(oA_IdxVec,i) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(yh_(oA_IdxVec,i), obs_pivotA);  <span class="comment">% add pivot angle back to calculate actual predicted mean</span>
0188     <span class="keyword">end</span>
0189 
0190     Py  = W(3)*temp2(:,1)*temp2(:,1)' + W(2)*temp2(:,2:nsp)*temp2(:,2:nsp)';
0191 
0192     <span class="comment">%------------------------------------------------------</span>
0193     <span class="comment">% MEASUREMENT UPDATE</span>
0194 
0195     Pxy = W(3)*temp1(:,1)*temp2(:,1)' + W(2)*temp1(:,2:nsp)*temp2(:,2:nsp)';
0196     KG = Pxy / Py;
0197 
0198 
0199     <span class="keyword">if</span> isempty(InferenceDS.innovation)
0200         inov(:,i) = obs(:,i) - yh_(:,i);
0201         <span class="keyword">if</span> ~isempty(oA_IdxVec)
0202           inov(oA_IdxVec,i) = <a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>(obs(oA_IdxVec,i), yh_(oA_IdxVec,i));
0203         <span class="keyword">end</span>
0204     <span class="keyword">else</span>
0205         inov(:,i) = InferenceDS.innovation( InferenceDS, obs(:,i), yh_(:,i));  <span class="comment">% inovation (observation error)</span>
0206     <span class="keyword">end</span>
0207 
0208 
0209     <span class="keyword">if</span> isempty(sA_IdxVec)
0210        xh(:,i) = xh_(:,i) + KG*inov(:,i);
0211     <span class="keyword">else</span>
0212        upd = KG*inov(:,i);
0213        xh(:,i) = xh_(:,i) + upd;
0214        xh(sA_IdxVec,i) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(xh_(sA_IdxVec,i), upd(sA_IdxVec));
0215     <span class="keyword">end</span>
0216 
0217     Px = Px_ - KG*Py*KG';
0218 
0219     state = xh(:,i);
0220     Pstate = Px;
0221 
0222     <span class="keyword">if</span> pNoise.adaptMethod <span class="keyword">switch</span> InferenceDS.inftype
0223     <span class="comment">%---------------------- UPDATE PROCESS NOISE SOURCE IF NEEDED --------------------------------------------</span>
0224     <span class="keyword">case</span> <span class="string">'parameter'</span>  <span class="comment">%--- parameter estimation</span>
0225         <span class="keyword">switch</span> pNoise.adaptMethod
0226         <span class="keyword">case</span> <span class="string">'anneal'</span>
0227             pNoise.cov = diag(max(pNoise.adaptParams(1) * diag(pNoise.cov) , pNoise.adaptParams(2)));
0228         <span class="keyword">case</span> <span class="string">'lambda-decay'</span>
0229             pNoise.cov = (1/pNoise.adaptParams(1)-1) * Pstate;
0230         <span class="keyword">case</span> <span class="string">'robbins-monro'</span>
0231             nu = 1/pNoise.adaptParams(1);
0232             pNoise.cov = (1-nu)*pNoise.cov + nu*KG*(KG*inov*inov')';
0233             pNoise.adaptParams(1) = min(pNoise.adaptParams(1)+1, pNoise.adaptParams(2));
0234         <span class="keyword">otherwise</span>
0235             error(<span class="string">' [ukf]unknown process noise adaptation method!'</span>);
0236         <span class="keyword">end</span>
0237 
0238         Sv = chol(pNoise.cov)';
0239 
0240     <span class="keyword">case</span> <span class="string">'joint'</span>  <span class="comment">%--- joint estimation</span>
0241         idx = pNoise.idxArr(<span class="keyword">end</span>,:); <span class="comment">% get indexs of parameter block of combo-gaussian noise source</span>
0242         ind1 = idx(1); ind2 = idx(2);
0243         idxRange = ind1:ind2;
0244         <span class="keyword">switch</span> pNoise.adaptMethod
0245         <span class="keyword">case</span> <span class="string">'anneal'</span>
0246             pNoise.cov(idxRange,idxRange) = diag(max(pNoise.adaptParams(1) * diag(pNoise.cov(idxRange,idxRange)), pNoise.adaptParams(2)));
0247         <span class="keyword">case</span> <span class="string">'lambda-decay'</span>
0248             param_length = ind2-ind1+1;
0249             pNoise.cov(idxRange,idxRange) = (1/pNoise.adaptParams(1)-1) * Pstate(end-param_length+1:<span class="keyword">end</span>,end-param_length+1:end);
0250         <span class="keyword">case</span> <span class="string">'robbins-monro'</span>
0251             param_length = ind2-ind1+1;
0252             nu = 1/pNoise.adaptParams(1);
0253             subKG = KG(end-param_length+1:<span class="keyword">end</span>,:);
0254             pNoise.cov(idxRange,idxRange) = (1-nu)*pNoise.cov(idxRange,idxRange) + nu*subKG*(subKG*inov*inov')';
0255             pNoise.adaptParams(1) = min(pNoise.adaptParams(1)+1, pNoise.adaptParams(2));
0256         <span class="keyword">otherwise</span>
0257             error(<span class="string">' [ukf]unknown process noise adaptation method!'</span>);
0258         <span class="keyword">end</span>
0259 
0260         Sv = chol(pNoise.cov)';
0261 
0262     <span class="comment">%--------------------------------------------------------------------------------------------------</span>
0263     <span class="keyword">end</span>; <span class="keyword">end</span>
0264 
0265 
0266 <span class="keyword">end</span>   <span class="comment">%--- for loop</span>
0267 
0268 
0269 <span class="keyword">if</span> (nargout&gt;4),
0270     InternalVariablesDS.xh_   = xh_;
0271     InternalVariablesDS.Px_   = Px_;
0272     InternalVariablesDS.yh_   = yh_;
0273     InternalVariablesDS.inov  = inov;
0274     InternalVariablesDS.Pinov = Py;
0275     InternalVariablesDS.KG    = KG;
0276 <span class="keyword">end</span>
0277 
0278</pre></div>
<hr><address>Generated on Tue 26-Sep-2006 10:36:21 by <strong><a href="http://www.artefact.tk/software/matlab/m2html/">m2html</a></strong> &copy; 2003</address>
</body>
</html>